#include "stdafx.h"
#include "Unit.h"

#include "Brick.h"
#include "PotentialField.h"

static float MAX_SPEED = 100.0f;
static float ROTATION_SPEED = 3.01f;

void Unit::reset( Brick & brick )
{
	mBrick = &brick;

	brick.Position = Position;
	brick.Orientation = Orientation;

	mBSphere = mmath::BoundingSphere(Position, brick.BSphereRadius);

	mHeadingToDest = false;
	mRotationY = 0; // might be taken from brick
}

void Unit::update( float dt )
{
	if(mHeadingToDest)
		headToDest(dt);

	mBrick->Position = Position;

	XMStoreFloat4(&Orientation, XMQuaternionRotationAxis(XMVectorSet(0,1,0,0), mRotationY));
	mBrick->Orientation = Orientation;

	mBSphere.Center = Position;
}

void Unit::setCurrentDestination( XMFLOAT3 dest )
{
	mCurrDest = dest;
	mHeadingToDest = true;
}

static void getAngleDiffAndDirToFastestMeet(float a0, float a1, float & diff, float & a0Dir)
{
	float diff0 = fabsf(a0 - a1);
	float diff1 = a0 + a1;
	diff = min(diff0, diff1);
	if((a0 > a1 && diff0 > diff1) || (a0 < a1 && diff0 < diff1))
		a0Dir = 1;
	else
		a0Dir = -1;
}

void Unit::headToDest(float dt)
{
	XMVECTOR position = XMVectorSetY(XMLoadFloat3(&Position), 0);
	XMVECTOR dest = XMVectorSetY(XMLoadFloat3(&mCurrDest), 0);

	XMVECTOR posToDest = XMVectorSubtract(dest, position);
	if(XMVectorGetX( XMVector3Length( posToDest ) ) < 2.5f)
	{
		mHeadingToDest = false;
		return;
	}

	XMVECTOR avoidForce = AI::getAvoidArmyUnitsPFForce(*this);
	XMVECTOR goalForce = AI::getLinearPotentialFieldForce(dest, position, 100.0f);

	XMVECTOR force = XMVectorAdd( avoidForce, goalForce );
	float forceLen = XMVectorGetX(XMVector3Length(force));
	
	XMVECTOR move = XMVectorScale( XMVectorScale( force , dt / forceLen), min(forceLen, MAX_SPEED) );

	position = XMLoadFloat3(&Position);
	position = XMVectorAdd( position, move );
	XMStoreFloat3(&Position, position);

	float moveRotY = atan2(XMVectorGetX(XMVector3Normalize(force)), XMVectorGetZ(XMVector3Normalize(force))) + PI;

	float angleDiff, turnDir;
	getAngleDiffAndDirToFastestMeet(mRotationY, moveRotY, angleDiff, turnDir);

	if(angleDiff > 0.1f)
	{
		mRotationY += ROTATION_SPEED * dt * turnDir;
		if(mRotationY >= 2*PI)
			mRotationY -= 2*PI;
		if(mRotationY < 0)
			mRotationY += 2*PI;
	}
}
